{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "from __future__ import division, print_function\n",
    "%matplotlib inline\n",
    "import sys\n",
    "sys.path.insert(0,'..') # allow us to format the book\n",
    "sys.path.insert(0,'../kf_book') \n",
    "# use same formattibng as rest of book so that the plots are\n",
    "# consistant with that look and feel.\n",
    "import book_format\n",
    "#book_format.load_style(directory='..')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "import numpy as np\n",
    "from numpy.random import randn, random, uniform, seed\n",
    "import scipy.stats\n",
    "\n",
    "class ParticleFilter(object):\n",
    "\n",
    "    def __init__(self, N, x_dim, y_dim):\n",
    "        self.particles = np.empty((N, 3))  # x, y, heading\n",
    "        self.N = N\n",
    "        self.x_dim = x_dim\n",
    "        self.y_dim = y_dim\n",
    "\n",
    "        # distribute particles randomly with uniform weight\n",
    "        self.weights = np.empty(N)\n",
    "        self.weights.fill(1./N)\n",
    "        self.particles[:, 0] = uniform(0, x_dim, size=N)\n",
    "        self.particles[:, 1] = uniform(0, y_dim, size=N)\n",
    "        self.particles[:, 2] = uniform(0, 2*np.pi, size=N)\n",
    "\n",
    "\n",
    "    def predict(self, u, std):\n",
    "        \"\"\" move according to control input u with noise std\"\"\"\n",
    "\n",
    "        self.particles[:, 2] += u[0] + randn(self.N) * std[0]\n",
    "        self.particles[:, 2] %= 2 * np.pi\n",
    "\n",
    "        d = u[1] + randn(self.N)\n",
    "        self.particles[:, 0] += np.cos(self.particles[:, 2]) * d\n",
    "        self.particles[:, 1] += np.sin(self.particles[:, 2]) * d\n",
    "\n",
    "        self.particles[:, 0:2] += u + randn(self.N, 2) * std\n",
    "\n",
    "\n",
    "    def weight(self, z, var):\n",
    "        dist = np.sqrt((self.particles[:, 0] - z[0])**2 +\n",
    "                       (self.particles[:, 1] - z[1])**2)\n",
    "\n",
    "        # simplification assumes variance is invariant to world projection\n",
    "        n = scipy.stats.norm(0, np.sqrt(var))\n",
    "        prob = n.pdf(dist)\n",
    "\n",
    "        # particles far from a measurement will give us 0.0 for a probability\n",
    "        # due to floating point limits. Once we hit zero we can never recover,\n",
    "        # so add some small nonzero value to all points.\n",
    "        prob += 1.e-12\n",
    "        self.weights += prob\n",
    "        self.weights /= sum(self.weights) # normalize\n",
    "\n",
    "\n",
    "    def neff(self):\n",
    "        return 1. / np.sum(np.square(self.weights))\n",
    "\n",
    "\n",
    "    def resample(self):\n",
    "        p = np.zeros((self.N, 3))\n",
    "        w = np.zeros(self.N)\n",
    "\n",
    "        cumsum = np.cumsum(self.weights)\n",
    "        for i in range(self.N):\n",
    "            index = np.searchsorted(cumsum, random())\n",
    "            p[i] = self.particles[index]\n",
    "            w[i] = self.weights[index]\n",
    "\n",
    "        self.particles = p\n",
    "        self.weights.fill(1.0 / self.N)\n",
    "\n",
    "\n",
    "    def estimate(self):\n",
    "        \"\"\" returns mean and variance \"\"\"\n",
    "        pos = self.particles[:, 0:2]\n",
    "        mu = np.average(pos, weights=self.weights, axis=0)\n",
    "        var = np.average((pos - mu)**2, weights=self.weights, axis=0)\n",
    "\n",
    "        return mu, var"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "image/png": "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\n",
      "text/plain": [
       "<Figure size 288x288 with 1 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from pf_internal import plot_pf\n",
    "\n",
    "seed(1234)\n",
    "N = 3000\n",
    "pf = ParticleFilter(N, 20, 20)\n",
    "xs = np.linspace (1, 10, 20)\n",
    "ys = np.linspace (1, 10, 20)\n",
    "zxs = xs + randn(20)\n",
    "zys = xs + randn(20)\n",
    "\n",
    "def animatepf(i):\n",
    "    if i == 0:\n",
    "        plot_pf(pf, 10, 10, weights=False)\n",
    "        \n",
    "    idx = int((i-1) / 3)\n",
    "    x, y = xs[idx], ys[idx]\n",
    "    z = [x + randn()*0.2, y + randn()*0.2]\n",
    "\n",
    "    step = (i % 3) + 1\n",
    "    if step == 2:\n",
    "        pf.predict((0.5, 0.5), (0.2, 0.2))\n",
    "        pf.weight(z=z, var=.6)\n",
    "        plot_pf(pf, 10, 10, weights=False)\n",
    "        plt.title('Step {}: Predict'.format(idx+1))\n",
    "    elif step == 3:\n",
    "        pf.resample()\n",
    "        plot_pf(pf, 10, 10, weights=False)\n",
    "        plt.title('Step {}: Resample'.format(idx+1))\n",
    "\n",
    "    else:\n",
    "        mu, var = pf.estimate()\n",
    "        plot_pf(pf, 10, 10, weights=False)\n",
    "        plt.scatter(mu[0], mu[1], color='g', s=100, label='PF')\n",
    "        plt.scatter(x, y, marker='x', color='r', s=180, lw=3, label='Robot')\n",
    "        plt.title('Step {}: Estimate'.format(idx+1))\n",
    "        #plt.scatter(mu[0], mu[1], color='g', s=100, label=\"PF\")\n",
    "        #plt.scatter([x+1], [x+1], marker='x', color='r', s=180, label=\"True\", lw=3)\n",
    "        plt.legend(scatterpoints=1, loc=2)\n",
    "    plt.tight_layout()\n",
    "\n",
    "from gif_animate import animate\n",
    "animate('particle_filter_anim.gif', animatepf, \n",
    "        frames=40, interval=800, figsize=(4, 4))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "<img src='particle_filter_anim.gif'>"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}
